#include "omni_wheel.h"
#include "omni_wheel_cal.h"
#include "M3508.h"
#include "dbus.h"
#include "interface.h"

Wheel_Data Small_cake;

void dbus_omni_wheel_speed()
{
	//dbus_ctrl_data.channel.CH[2]=1122;
	Small_cake.Robot_in_world.velocity_exp.Vx = (dbus_ctrl_data.channel.CH[2]-0x400)*6;
	Small_cake.Robot_in_world.velocity_exp.Vy = (dbus_ctrl_data.channel.CH[3]-0x400)*6;
	Small_cake.Robot_in_world.velocity_exp.Vw = (dbus_ctrl_data.channel.rolling_wheel-0x400)*12;
	Small_cake.Robot_in_world.position_exp.yaw = (dbus_ctrl_data.channel.CH[0]-0x400)*180/660;
	//Pos = ((float)dbus_ctrl_data.channel.CH[0]-364)*8191/1320;
}

void omni_wheel_execute()
{
	dbus_omni_wheel_speed();
	World_To_Self(&Small_cake);
	Robot_Velocity_To_Wheel_Speed(&Small_cake);
	Wheel_Data_To_Motor(&Small_cake,&M3508_Motor);
	speed_ctrl(&M3508_Motor,M3508_Motor.M3508[0].exp_speed,M3508_Motor.M3508[1].exp_speed,M3508_Motor.M3508[2].exp_speed,M3508_Motor.M3508[3].exp_speed,DJI_M3508);
}